clear all;
close all;

J=2;R=3;l1=1;l2=0.01;l=0.01;lamb2=1;

Qk=[1;0];% ode45不能直接用矩阵，得变成向量进行操作；依次是phi,th,fi,h
Zk=[0;0];

T=0.001;
for k=1:1:30000

    time(k)=k*T;
    xd=0;

    if(k*T>=15)
        dt=0.2*cos(2*pi/3*k*T);
        %dt=0.2;
    else
        dt=0;
    end
    
    x1=Qk(1);x2=Qk(2);
    n1=Zk(1);k1=Zk(2);
    x(k)=x1;
    
    e(k)=x1-xd;
    D = k1 + lamb2*J*x2;
    s = R*(x1-xd)+x2;
    
    Ueq =-(1/J)*R*x2;
    Usw =-(1/J)*(l1*s+l2*(s/(abs(s)+l)))-D;
    U = Ueq + Usw;
    ut(k)=U;

    tSpan = [0 T];
	[tt,Qq] = ode45(@(t,x) plant1(t,x,U,dt),tSpan,Qk);
    Qk = Qq(length(Qq),:);
	[tt1,Zq] = ode45(@(t,x) obv1(t,x,U,D,x2),tSpan,Zk);
    Zk = Zq(length(Zq),:);
    edt(k)=D-dt;
end

figure(1);
plot(time,x,'k');
xlabel('time(s)');ylabel('th');
legend('x1');
grid on;

figure(2);
plot(time,ut,'k');
xlabel('time(s)');ylabel('U');
legend('U1');
 
figure(3);
plot(time,edt,'k');
xlabel('time(s)');ylabel('edt');
legend('edt');
